﻿#coding: utf-8

from math import sqrt

n=5
xi = [2,2,2,2,2]
yi = [2,2,2,2,2]
zi = [2,2,2,2,2]
ri = [6, 7, 2, 3, 4]

def soma_v(v1,v2):
	res = [0]*len(v1)
	for i in range(0,len(v1)):
		res[i] = v1[i]+v2[i]
	return res
	
def interno_v(v1,v2):
	res = 0
	for i in range(0,len(v1)):
		res += v1[i]*v2[i]
		
	return res	
	
def escalar_v(v1,k):
	res = [0]*len(v1)
	for i in range(0,len(v1)):
		res[i] = v1[i]*k
		
	return res	
	
def norma(v):
	res = 0
	for i in range(0,len(v)):
		res+=pow(v[i],2)
	res = sqrt(res)
	return res
	
def Ei(i,v,x,y,z):
	ind=3*i
	return pow( (x-v[ind]),2 ) + pow( (y-v[ind+1]),2 ) + pow( (z-v[ind+2]),2 ) - pow(ri[i],2)
	
def R(e,v):
	ind=3*e
	res = 0
	for i in range(0,n):
		if i!=e:
			res+=pow( min( Ei(i,v,v[ind],v[ind+1],v[ind+2]),0 ),2 )
			
	return res
	
def F(v):
	res = 0
	for i in range(0,n):
		ind = 3*i
		res+=pow(v[ind],2)+pow(v[ind+1],2)+pow(v[ind+2],2) + 1000000*R(i,v)

	return res
	
def grad_F(v):
	grad = [0]*3*n
	for i in range(0,n):
		ind = 3*i
		grad[ind] = 2*v[ind]
		grad[ind+1] = 2*v[ind+1]
		grad[ind+2] = 2*v[ind+2]
		
	return grad
	
def v_from_vec():
	v = [0]*3*n
	for i in range(0,n):
		ind = 3*i
		v[ind] = xi[i]
		v[ind+1] = yi[i]
		v[ind+2] = zi[i]
		
	return v	
	
def busca_linear(xk,dk):
	sigma = 0.8
	lbda = 1.0
	lim = 10	
	
	while( F( soma_v(xk,escalar_v(dk,lbda)) ) >= F(xk) + sigma*lbda*interno_v(grad_F(xk),dk) ):
		if lim == 0:
			break			
		lbda/=2
		lim-=1
	
	return lbda
		
def main():
	x0 = v_from_vec()
	epson = 1e-2
	it = 0
	
	while(norma(x0)>epson):
		if it==700:
			break
			
		dk = escalar_v(grad_F(x0),-1)
		lbda = busca_linear(x0,dk)
		x0 = soma_v(x0,escalar_v(dk,lbda))
		
		it+=1
	
	for i in range(0,n):
		ind = 3*i
		print "P",i,"= ( ",x0[ind],x0[ind+1],x0[ind+2],ri[i]," )"
	
	print it
	
main()
